package frc2856.robot;

import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.SpeedController;

/**
 *
 * @author Robert
 */
public class Shooter {

    public Encoder frontEnc, backEnc;
    public PIDController frontPID, backPID;
    public SpeedController frontMotor, backMotor;
    public Solenoid pusherA, pusherB;
    public double distancePerPulse = 0.0;
    public double defaultSpeed = .5, defaultOutputRange = .75;
    public double P, I, D;
    boolean pidEnabled;
    boolean shooterEnabled;

    public Shooter(boolean pid) {
        pidEnabled = pid;
        shooterEnabled = false;
        
        frontMotor = new Jaguar(RobotMap.shtr_jagF_mod, RobotMap.shtr_jagF_chan);
        backMotor = new Jaguar(RobotMap.shtr_jagB_mod, RobotMap.shtr_jagB_chan);
        pusherA = new Solenoid(RobotMap.shtr_solA_mod, RobotMap.shtr_solA_chan);
	pusherB = new Solenoid(RobotMap.shtr_solB_mod, RobotMap.shtr_solB_chan);
	
        if (pidEnabled) {
            P = 0.03;
            I = 0.003;
            D = 0.0;
            frontEnc = new Encoder(RobotMap.shtr_encF_mod, RobotMap.shtr_encF_chan_A, RobotMap.shtr_encF_mod, RobotMap.shtr_encF_chan_B, RobotMap.shtr_encF_dirc, EncodingType.k1X);
            backEnc = new Encoder(RobotMap.shtr_encB_mod, RobotMap.shtr_encB_chan_A, RobotMap.shtr_encB_mod, RobotMap.shtr_encB_chan_B, RobotMap.shtr_encB_dirc, EncodingType.k1X);
            frontEnc.setDistancePerPulse(distancePerPulse);
            backEnc.setDistancePerPulse(distancePerPulse);
            frontEnc.start();
            backEnc.start();
            frontEnc.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);
            backEnc.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);

            frontPID = new PIDController(P, I, D, frontEnc, frontMotor, PIDController.kDefaultPeriod);
            backPID = new PIDController(P, I, D, backEnc, frontMotor, PIDController.kDefaultPeriod);
            frontPID.setContinuous();
            backPID.setContinuous();
            frontPID.setOutputRange(0.0, defaultOutputRange);
            backPID.setOutputRange(0.0, defaultOutputRange);
        }
    }

    public void spinStop() {
        spinEffort(0, 0);
    }

    public void startDefaultSpeed() {
        frontMotor.set(defaultSpeed);
        backMotor.set(defaultSpeed);
    }

    public void spinEffort(double effort) {
        spinEffort(effort, effort);
    }

    public void spinEffort(double front, double back) {
        frontMotor.set(front);
        backMotor.set(back);
    }
    
    public double getEffort(){
        return frontMotor.get();
    }

    public void shoot(boolean open) {
        pusherA.set(!open);
	pusherB.set(open);
    }
    
    public void unshoot(){
        this.shoot(false);
    }

    public double[] getEncoderDistance() {
        if (pidEnabled) {
            return new double[]{frontEnc.getDistance(), backEnc.getDistance()};
        } else {
            return new double[]{-1, -1};
        }
    }

    public double[] getEncoderRate() {
        if (pidEnabled) {
            return new double[]{frontEnc.getRate(), backEnc.getRate()};
        } else {
            return new double[]{-1, -1};
        }
    }

    public double[] getPID() {
        if (pidEnabled) {
            return new double[]{frontPID.get(), backPID.get()};
        } else {
            return new double[]{-1, -1};
        }
    }

    public void setOutputRange(double output) {
        if (pidEnabled) {
            frontPID.setOutputRange(0.0, output);
            backPID.setOutputRange(0.0, output);
        }
    }

    public void spinRate(double front, double back) {
        if (pidEnabled) {
            frontPID.reset();
            backPID.reset();
            frontPID.setSetpoint(front);
            backPID.setSetpoint(back);
            frontPID.enable();
            backPID.enable();
        }
    }

    public void spinRateUpdate(double rate) {
        if (pidEnabled) {
            spinRateUpdate(rate, rate);
        }
    }

    public void spinRateUpdate(double front, double back) {
        if (pidEnabled) {
            if (frontPID.isEnable() || backPID.isEnable()) {
                frontPID.setSetpoint(front);
                backPID.setSetpoint(back);
            } else {
                spinRate(front, back);
            }
        }
    }

    public double[] decrementP() {
        double[] returnVal = new double[]{-1, -1};
        if (pidEnabled) {
            double p;
            p = frontPID.getP() - 0.01;
            frontPID.setPID(p, frontPID.getI(), frontPID.getD());
            returnVal[0] = p;
            p = backPID.getP() - 0.01;
            backPID.setPID(p, backPID.getI(), backPID.getD());
            returnVal[1] = p;
        }
        return returnVal;
    }

    public double[] incrementP() {
        double[] returnVal = new double[]{-1, -1};
        if (pidEnabled) {
            double p;
            p = frontPID.getP() + 0.01;
            frontPID.setPID(p, frontPID.getI(), frontPID.getD());
            returnVal[0] = p;
            p = backPID.getP() + 0.01;
            backPID.setPID(p, backPID.getI(), backPID.getD());
            returnVal[1] = p;
        }
        return returnVal;
    }

    public double[] decrementI() {
        double[] returnVal = new double[]{-1, -1};
        if (pidEnabled) {
            double i;
            i = frontPID.getP() - 0.01;
            frontPID.setPID(frontPID.getP(), i, frontPID.getD());
            returnVal[0] = i;
            i = backPID.getP() - 0.01;
            backPID.setPID(backPID.getP(), i, backPID.getD());
            returnVal[1] = i;
        }
        return returnVal;
    }

    public double[] incrementI() {
        double[] returnVal = new double[]{-1, -1};
        if (pidEnabled) {
            double i;
            i = frontPID.getP() + 0.01;
            frontPID.setPID(frontPID.getP(), i, frontPID.getD());
            returnVal[0] = i;
            i = backPID.getP() + 0.01;
            backPID.setPID(backPID.getP(), i, backPID.getD());
            returnVal[1] = i;
        }
        return returnVal;
    }

    public double[] decrementD() {
        double[] returnVal = new double[]{-1, -1};
        if (pidEnabled) {
            double d;
            d = frontPID.getD() - 0.01;
            frontPID.setPID(frontPID.getP(), frontPID.getI(), d);
            returnVal[0] = d;
            d = backPID.getP() - 0.01;
            backPID.setPID(backPID.getP(), backPID.getI(), d);
            returnVal[1] = d;
        }
        return returnVal;
    }

    public double[] incrementD() {
        double[] returnVal = new double[]{-1, -1};
        if (pidEnabled) {
            double d;
            d = frontPID.getD() + 0.01;
            frontPID.setPID(frontPID.getP(), frontPID.getI(), d);
            returnVal[0] = d;
            d = backPID.getP() + 0.01;
            backPID.setPID(backPID.getP(), backPID.getI(), d);
            returnVal[1] = d;
        }
        return returnVal;
    }
    
    public void toggleShooter(){
        if(shooterEnabled){
            spinEffort(0);
        }
        shooterEnabled = !shooterEnabled;
    }
}